#include <ros/ros.h>

#include "cg_interfaces/ChassisStruct.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "chassis_struct_pub");
    ros::NodeHandle nh;
    ros::Publisher chassis_struct_pub = nh.advertise<cg_interfaces::ChassisStruct>("chassis_struct_rc", 1000);

    std::string init_str;
    ros::NodeHandle nlocal("~");
    if (!nlocal.getParam("init_str", init_str))
    {
        ros::shutdown();
        return -1;
    }

    cg_interfaces::ChassisStruct init;
    int index = init_str.find_first_of(':');
    init.master = std::stoi(init_str.substr(0, index));
    int count = 0;
    while (true)
    {
        int index2 = init_str.find_first_of(',', index + 1);
        if (index2 == -1)
        {
            break;
        }
        init.struct_ids[count] = std::stoi(init_str.substr(index + 1, index2 - index - 1));
        count++;
        index = index2;
    }
    init.struct_ids[count] = std::stoi(init_str.substr(index + 1));
    init.count = count + 1;

    ros::Rate rate(1);
    while (ros::ok())
    {
        chassis_struct_pub.publish(init);
        ros::spinOnce();
        rate.sleep();
    }
}